#include "utils.h"
namespace control
{



Track_mag::Track_mag( double offset_y,double max_speed, double &vel_cmd, double cycle_time,int stop_step, PIDControl& pid_control_offsety ,ControlParamType controlParamInfo)
    :offset_y_(offset_y),max_speed_(max_speed), vel_cmd_(vel_cmd),cycle_time_(cycle_time),stop_step_(stop_step),pid_control_offsety_(pid_control_offsety),controlParamInfo_(controlParamInfo)
{
    //for test
    double dt         = cycle_time_;
    double radius     = 0.0;
    double current_target_dis = 0.0;
    int index_nearest = 0;
   // RCLCPP_INFO(node->get_logger(),"max speed : %f,cycle_time_ : %f,acc : %f,",max_speed_,cycle_time_,controlParamInfo_.acc_limit);
    //RCLCPP_INFO(node->get_logger()," offset_y_ : %f,controlParamInfo.adaptive_kp_line : %f,",offset_y_,controlParamInfo.adaptive_kp_line);
    w_cmd_   = PIDController( offset_y_, controlParamInfo_, dt, pid_control_offsety);
    vel_cmd_ = VelocityPlan("LINE",max_speed_,vel_cmd_,stop_step_, controlParamInfo_.lift_flag);
    // if(controlParamInfo_.lift_flag==1)
    // {
    //     vel_cmd_ = VelocityPlan("LINE",max_speed_,vel_cmd_,stop_step_, controlParamInfo_.acc_limit);
    // }
    // else if(controlParamInfo_.lift_flag==2)
    // {
    //     vel_cmd_ = VelocityPlan("LINE",max_speed_,vel_cmd_,stop_step_, controlParamInfo_.acc_limit_w);
    // }
    if(vel_cmd_ < 0.01)
    {
        w_cmd_ = 0;
    }
    //RCLCPP_INFO(node->get_logger()," vel_cmd_ : %f,w_cmd_ : %f,",vel_cmd_,w_cmd_);

    left_speed_ = vel_cmd_ + w_cmd_;
    right_speed_ = vel_cmd_ - w_cmd_;
}

double Track_mag::VelocityPlan(std::string path_type,double &max_speed ,double &vel_cmd,int &stop_step,int &lift_flag)
{
    double vt=vel_cmd;
    if(path_type == "LINE")
    {
        if(max_speed < EPSILON && vel_cmd <0.02)
        {
            vt=0;
            return vt;
        }
        if(stop_step == ESTOP)
        {
            if(vel_cmd_ > controlParamInfo_.estop_speed_level + EPSILON)
            {
                vt = vel_cmd_ - controlParamInfo_.obs_estop_dec  * cycle_time_;
                vt = vt <= controlParamInfo_.estop_speed_level ? controlParamInfo_.estop_speed_level : vt;
            }
            else
            {
                vt = vel_cmd_ - controlParamInfo_.obs_brake_dec  * cycle_time_;
                vt = vt < 0.05 ? 0.0 : vt;
            }
        }
        else
        {
            double acc = (lift_flag == 1) ? controlParamInfo_.acc_limit:controlParamInfo_.acc_limit_w;
            double acc_start = (lift_flag == 1) ? controlParamInfo_.start_acc_limit:controlParamInfo_.start_acc_limit_w;
            if(max_speed >= vel_cmd)
            {
                vt=vel_cmd + (vel_cmd < controlParamInfo_.start_speed_limit ? cycle_time_*acc_start : cycle_time_*acc);
            }
            else
            {
                vt=vel_cmd-cycle_time_*acc;
            }
            if(stop_step  == BRAKE)
            {
                double vt_break;
                vt_break = vel_cmd_ - controlParamInfo_.obs_brake_dec  * cycle_time_;

                double target_break_speed = max_speed < 0.3 ? max_speed : 0.3;
                vt_break = vt_break < target_break_speed ? target_break_speed : vt_break;
                if(vt_break < vt)
                {
                    vt = vt_break;
                }
            }
        }

    }

    return vt;
}

double Track_mag::PIDController(double offset_y,ControlParamType controlParamInfo,double dt,PIDControl& pid_control_offsety)
{
    //在到达目标点的最后30cm这段距离，要进行精细控制，控制参数调大，尽快消除横向偏差
    //两种方式来消除横向偏差：（1）调大转向停止角度（2）控制参数调大，增大旋转角速度命令
    //设计两个参数（1.停止角度，2.控制参数）验证效果
    std::pair<double,double> cmd_w;
    double w1 = 0.0,w2 = 0.0;
    PIDConf pid_conf_offsety;
    
    // if (path_.PathType == "LINE")
    if(1)
    {
        pid_conf_offsety.kp  = controlParamInfo.adaptive_kp_line; 
        pid_conf_offsety.ki  = controlParamInfo.adaptive_ki_line;
        pid_conf_offsety.kd  = controlParamInfo.adaptive_kd_line;
    }
    pid_control_offsety.setPID(pid_conf_offsety);
    w1 = pid_control_offsety.control(offset_y_,dt);
    // if(fabs(offset_y_) < 0.015)
    // {
    //     w1 = pid_control_offsety.control(offset_y_,dt)*fabs(0.015/0.035); 
    // }
    // else
    // {
    //     w1 = pid_control_offsety.control(offset_y_,dt)*fabs(offset_y_/0.035); 
    // }

    
    return w1;
}


double Track_mag::getVelCmd()
{
    return vel_cmd_;
}

double Track_mag::getWCmd()
{
    return w_cmd_;
}


int Track_mag::getStopStep()
{
    return stop_step_;
}


double Track_mag::getLeftSpeed()
{
    return left_speed_;
}

double Track_mag::getRightSpeed()
{
    return right_speed_;
}


void Track_mag::Clear()
{
    offset_y_              = 0.0;
    vel_cmd_               = 0.0;
    w_cmd_                 = 0.0;
    stop_step_             = 0;
    right_speed_           = 0.0;
    left_speed_            = 0.0;
}


}



